Celestin Apprentice 7/Apprentice-Release7.iso/Source Code/C ++/Applications/FlyThrough 1.1.2/src/Source/Q3Utilities.cp

Text File | 1997-03-10 | 4.8 KB | 149 lines | [TEXT/CWIE]

//
//    Q3Utilities.cp
//
//    A bunch of handy QD3D utilites, mostly math.
//    These are written as if they were natural extensions of the QD3D API.
//
//    by James Jennings
//    March 9, 1997
//

#include "Q3Utilities.h"

TQ3Boolean Q3::Vector3D_Parallel( const TQ3Vector3D *v1, const TQ3Vector3D *v2 )
{
    // Return kQ3True if v1 and v2 are parallel.
    // also if either are null.
    TQ3Vector3D u;
    ::Q3Vector3D_Cross( v1, v2, &u );
    return (::Q3Vector3D_Length( &u ) <= kQ3RealZero) ? kQ3True : kQ3False;
}

TQ3Boolean Q3::Vector3D_Orthogonal( const TQ3Vector3D *v1, const TQ3Vector3D *v2 )
{
    // Return kQ3True if v1 and v2 are orthogonal.
    // also if either are null.
    return ( ::Q3Vector3D_Dot( v1, v2 ) <= kQ3RealZero ) ? kQ3True : kQ3False;
}

TQ3Vector3D * Q3::Vector3D_MakeOrthogonal(
        const TQ3Vector3D *v1, const TQ3Vector3D *v2, TQ3Vector3D *v3)
{
    // Make v2 orthogonal to v1 and return it in v3
    Assert_( v1!=nil && v2!=nil && v3!=nil );
    Assert_( ::Q3Vector3D_Length(v1) > 0 && ::Q3Vector3D_Length(v2) > 0 );
    Assert_( ! Vector3D_Parallel( v1, v2 ) );

    // Since v1, v2, and v3 might point at the same place in memory we store
    // intermediate results in v until we don't need v1 or v2 any more.
    TQ3Vector3D v;

    float dot = ::Q3Vector3D_Dot( v1, v2 );
    if (dot == 0.0) {
        v = *v2;    // already orthogonal
    } else {
        // v3 = v2 - (v1.v2)v1/|v1||v2|
        // Don't assume that v1 and v2 are normalized.
        dot /= ::Q3Vector3D_Length(v1) * ::Q3Vector3D_Length(v2);
        ::Q3Vector3D_Scale( v1, dot, &v );
        ::Q3Vector3D_Subtract( v2, &v, &v );
        Assert_(::Q3Vector3D_Length(&v) > 0 /* v1, v2 must not be parallel */);
    }

    ::Q3Vector3D_Normalize( &v, v3 );

    return v3;
}

TQ3Vector3D * Q3::Vector3D_CompleteBasis(
        const TQ3Vector3D *v1, const TQ3Vector3D *v2, TQ3Vector3D *v3)
{
    // Make v3 orthogonal to v1 and v2
    Assert_(v1!=nil && v2!=nil && v3!=nil);

    TQ3Vector3D v;
    ::Q3Vector3D_Cross( v1, v2, &v );
    Assert_( ::Q3Vector3D_Length( &v ) != 0 );

    ::Q3Vector3D_Normalize( &v, v3 );

    return v3;
}

TQ3CameraPlacement * Q3::CameraPlacement_Transform(
    const TQ3CameraPlacement *inPlace, const TQ3Matrix4x4 *M, TQ3CameraPlacement *outPlace )
{
    Assert_( inPlace != nil && M != nil && outPlace != nil);
    ::Q3Point3D_Transform( &inPlace->cameraLocation, M, &outPlace->cameraLocation );
    ::Q3Point3D_Transform( &inPlace->pointOfInterest, M, &outPlace->pointOfInterest );
    ::Q3Vector3D_Transform( &inPlace->upVector, M, &outPlace->upVector );
}

TQ3Boolean Q3::CameraPlacement_Validate( TQ3CameraPlacement *inPlace )
{
    // Test if the camera placement is reasonable, fixing the upVector if necessary.
    TQ3Vector3D v;
    ::Q3Point3D_Subtract( &inPlace->pointOfInterest, &inPlace->cameraLocation, &v );

    if ( ::Q3Vector3D_Length( &v ) <= kQ3RealZero ) return kQ3False;
    if ( ::Q3Vector3D_Length( &inPlace->upVector ) <= kQ3RealZero ) return kQ3False;
    if ( Vector3D_Parallel( &inPlace->upVector, &v ) ) return kQ3False;

    Vector3D_MakeOrthogonal( &v, &inPlace->upVector, &inPlace->upVector );

    return kQ3True;
}



TQ3Matrix4x4 *    Q3::Transform_SetRotatePlacementToPlacement(
    TQ3Matrix4x4 *M, const TQ3CameraPlacement *in1, const TQ3CameraPlacement *in2 )
{
    // Find a transformation that takes in1 to in2 (••• Not well tested •••)

    Assert_( M != nil && in1 != nil && in2 != nil );
    TQ3Matrix4x4 t;

    // Move to origin
    ::Q3Matrix4x4_SetTranslate( M,
            - in1->cameraLocation.x, - in1->cameraLocation.y, - in1->cameraLocation.z);
    // Aim
    TQ3Vector3D v1,v2,v3,v4;
    ::Q3Point3D_Subtract(&in1->pointOfInterest, &in1->cameraLocation, &v1);
    ::Q3Point3D_Subtract(&in2->pointOfInterest, &in2->cameraLocation, &v2);
    Assert_(::Q3Vector3D_Length(&v1) != 0 && ::Q3Vector3D_Length(&v2) != 0);
    ::Q3Vector3D_Normalize(&v1, &v1);
    ::Q3Vector3D_Normalize(&v2, &v2);
    if ( ::Q3Vector3D_Dot( &v1, &v2 ) == -1 ) {
        // v1 == -v2 then rotate 180° about an upVector. (Which one? I'm guessing in1.)
        const TQ3Point3D origin = {0,0,0};
        ::Q3Matrix4x4_SetRotateAboutAxis( &t, &origin, &in1->upVector, kQ3Pi );
    } else {
        // Else a vector to vector rotation is well defined.
        ::Q3Matrix4x4_SetRotateVectorToVector( &t, &v1, &v2 );
    }
    ::Q3Matrix4x4_Multiply( M, &t, M );

    // Roll
    // Assert: the upVectors are normalized
    v3 = in1->upVector;
    v4 = in2->upVector;
    ::Q3Vector3D_Transform( &v3, &t, &v3 );
    ::Q3Vector3D_Transform( &v4, &t, &v4 );
    if ( ::Q3Vector3D_Dot( &v3, &v4 ) == -1 ) {
        // v1 == -v2 then rotate 180° about the forward direction
        const TQ3Point3D origin = {0,0,0};
        ::Q3Matrix4x4_SetRotateAboutAxis( &t, &origin, &v2, kQ3Pi );
    } else {
        // Else a vector to vector rotation is well defined.
        ::Q3Matrix4x4_SetRotateVectorToVector( &t, &v3, &v4 );
    }
    ::Q3Matrix4x4_Multiply( M, &t, M );

    // Move to target
    ::Q3Matrix4x4_SetTranslate( &t,
            in2->cameraLocation.x, in2->cameraLocation.y, in2->cameraLocation.z);
    ::Q3Matrix4x4_Multiply( M, &t, M );
}